/*
* @File Name router.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\task\task_router\router.c
* @Author: Ruige_Lee
* @Date:   2019-07-29 23:49:15
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-08-29 19:24:59
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/

#include "stdint.h"
#include "route/trajectory/trajectory.h"

extern int32_t AStar_excuse(uint16_t startPoseNum, uint16_t destPoseNum, uint16_t router[100], uint16_t *edgeCnt);
extern float poseTable[2431][6];
void task_router()
{
//	uint16_t edgeCnt = 0;
//	uint16_t router[100] = {0};
//
//	AStar_excuse(10, 186, router, &edgeCnt);
//
//
//
//	float startConfig[6];
//	float endConfig[6];
//	float step[6];
//	//get perious place
//
//	for (uint8_t axis = 0; axis < 6; axis ++)
//	{
//		startConfig[axis] = stepper_getRad( 0, axis );
//
//		uint16_t pNum = router[edgeCnt];
//		endConfig[axis] = poseTable[pNum][axis];
//		step[axis] = (endConfig[axis] - startConfig[axis]) / 20;
//	}
//
//
//	//move to start pose
//	Cspline[0].count = 20;
//	for ( uint8_t p = 0; p < 20; p++ )
//	{
//		for (uint8_t axis = 0; axis < 6; axis ++)
//		{
//
//			Cspline[0].points[p].position[axis] = startConfig[axis] + p * (step[axis]);
//		}
//		Cspline[0].points[p].timePoint = 0.2*p;
//	}
//
//	tra_checkIn(0);
//	while( FIFO_empty[0] != 0 )
//		{;}
//	while( FIFO_empty[0] != 1 )
//	{;}
//
//
//
//
//
//
//	// 从router生成轨迹
//
//	Cspline[0].count = edgeCnt;
//
//	for ( uint16_t i = edgeCnt; i > 0; i-- )
//	{
//		uint16_t pNum = router[i];
//		for ( uint8_t axis = 0; axis < 6; axis ++ )
//		{
//			Cspline[0].points[edgeCnt - i].position[axis] = poseTable[pNum][axis];
//		}
//		Cspline[0].points[edgeCnt - i].timePoint = 0.2*(edgeCnt - i);
//	}
//
//	tra_checkIn(0);
}



